#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/float64_multi_array.hpp>
#include <vector>

class ObstaclePublisher : public rclcpp::Node
{
public:
    ObstaclePublisher()
    : Node("obstacle_publisher")
    {
        // 声明参数，包括cal_obs_param和用于返回的vector<double>参数
        this->declare_parameter<std::vector<double>>("cal_obs_param", std::vector<double>{});
        this->declare_parameter<std::vector<double>>("calculated_obstacles", std::vector<double>{});

        // 获取初始参数值
        cal_obs_param_ = this->get_parameter("cal_obs_param").as_double_array();
        obs_num_ = static_cast<int>(cal_obs_param_[0]);

        // 更新障碍物数据
        update_obstacles();

        // 添加参数回调函数以监听参数变化
        this->add_on_set_parameters_callback(
            std::bind(&ObstaclePublisher::on_parameters_set, this, std::placeholders::_1)
        );

        // 创建定时器每隔1秒发布一次数据
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1), 
            std::bind(&ObstaclePublisher::publish_data, this)
        );

        // 创建发布者
        publisher_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("obstacle_pub_topic", 10);
    }

private:
    // 参数更新回调
    rcl_interfaces::msg::SetParametersResult on_parameters_set(
        const std::vector<rclcpp::Parameter> &parameters)
    {
        for (const auto &param : parameters) {
            if (param.get_name() == "cal_obs_param") {
                cal_obs_param_ = param.as_double_array();
                obs_num_ = static_cast<int>(cal_obs_param_[0]);

                // 更新障碍物点数据
                update_obstacles();
            }
        }

        // 确保参数设置操作成功
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        return result;
    }

    // 更新障碍物数据
    void update_obstacles()
    {
        calculated_obstacles_.clear();  // 清空之前计算的结果

        for (size_t i = 1; i < obs_num_; i += 7) {
            int shape = static_cast<int>(cal_obs_param_[i]);
            double pos_x = cal_obs_param_[i + 1];
            double pos_y = cal_obs_param_[i + 2];
            double pos_z = cal_obs_param_[i + 3];
            double paraA = cal_obs_param_[i + 4];
            double paraB = cal_obs_param_[i + 5];
            double paraC = cal_obs_param_[i + 6];

            // 计算并存储结果
            calculated_obstacles_.push_back(pos_x);
            calculated_obstacles_.push_back(pos_y);
            calculated_obstacles_.push_back(pos_z);
            calculated_obstacles_.push_back(paraA);
            calculated_obstacles_.push_back(paraB);
            calculated_obstacles_.push_back(paraC);
        }

        // 更新ROS中的参数
        this->set_parameter(rclcpp::Parameter("calculated_obstacles", calculated_obstacles_));
    }

    // 定时器回调：发布数据
    void publish_data()
    {
        // 生成发布的数据
        std_msgs::msg::Float64MultiArray msg;
        for (const double &val : calculated_obstacles_) {
            msg.data.push_back(val);
        }

        // 发布数据
        publisher_->publish(msg);
    }

    // 参数和定时器相关变量
    std::vector<double> cal_obs_param_;
    int obs_num_;
    std::vector<double> calculated_obstacles_;  // 存储计算后的障碍物参数
    rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ObstaclePublisher>());
    rclcpp::shutdown();
    return 0;
}
